package fr.ants.model.ai.pathfinding;

import java.util.Comparator;
import java.util.HashSet;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.Set;
import java.util.Stack;

import fr.ants.model.Grid;
import fr.ants.model.Node;
import fr.ants.model.ai.heuristic.IHeuristic;

/** 
 * <p>Path finding with A* version.</p>
 *
 * @author Vianney DUPOY DE GUITARD
 */
public class AStar extends AntPathFinder {

	private IHeuristic heuristic;

	/** {@inheritDoc} */
	public Stack<Node> getPath(Grid grid, int x1, int y1, int x2, int y2) {
		// Set of opened nodes, that is to say the nodes to evalue.
		Queue<Node> openedSet = new PriorityQueue<>(2, new Comparator<Node>() {
			@Override
			public int compare(Node o1, Node o2) {
				// Sorts the nodes.
				return (int) (o1.getAStarData().getF() - o2.getAStarData().getF());
			}
		});
		// Set of closed nodes, that is to say the nodes already evaluated.
		Set<Node> closedSet = new HashSet<>();
		double sqrt2 = Math.sqrt(2);

		// The start & end nodes.
		Node startNode = grid.getNode(x1, y1);
		Node endNode = grid.getNode(x2, y2);

		// Our path will start with the start node.
		openedSet.add(startNode);
		startNode.getAStarData().setG(0);
		startNode.getAStarData().setF(heuristic.evaluate(x1, y1, x2, y2));

		// While there are nodes to evaluate.
		while (!openedSet.isEmpty()) {
			// Take the node with the lowest F score.
			Node curNode = openedSet.poll();

			// Check if we've reach the end node.
			if (endNode.equals(curNode)) {
				Stack<Node> backtrace = backtrace(curNode);
				grid.clearAStar();
				return backtrace;
			}

			// Sets this node as "evaluated".
			closedSet.add(curNode);

			// Browse each neighbor of this node.
			for (Node neighbor : grid.getNeighbors(curNode)) {
				if (closedSet.contains(neighbor)) {
					continue;
				}
				double nextG = neighbor.getAStarData().getG() + sqrt2 + neighbor.getDifficulty();
				if (!openedSet.contains(neighbor) || nextG < neighbor.getAStarData().getG()) {
					neighbor.getAStarData().setParent(curNode);
					neighbor.getAStarData().setG(nextG);
					neighbor.getAStarData().setF(nextG + heuristic.evaluate(neighbor.getX(), neighbor.getY(), x2, y2));
					if (!openedSet.contains(neighbor)) {
						openedSet.add(neighbor);
					}
				}
			}
		}

		// fail to find the path
		return null;
	}

	private Stack<Node> backtrace(Node node) {
		Stack<Node> stack = new Stack<>();
		stack.push(node);
		while (node.getAStarData().getParent() != null) {
			node = node.getAStarData().getParent();
			stack.push(node);
		}
		return stack;
	}

	/** {@inheritDoc} */
	public void setHeuristic(IHeuristic heuristic) {
		this.heuristic = heuristic;
	}
}